function [ C0 ] = dcmnormortho_optimized( CHat ) %#codegen
%DCMNORMORTHO_OPTIMIZED 对方向余弦矩阵进行规范正交化，按精确的最优正交化方法
%
% Input Arguments:
% # CHat: 3*3矩阵，方向余弦矩阵
%
% Output Arguments:
% # C0: 3*3矩阵，正交化后的方向余弦矩阵
%
% References:
% # 袁信, 郑谔. 捷联式惯性导航原理[M]. 北京: 航空专业教材编审室, 1985

C0 = real(sqrtm(CHat*CHat')) \ CHat; % REF1 6-9式
end